{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Estimating covariance for ICP\n",
    "\n",
    "Here is a small reminder how to implement the covariance matrix for the ICP algorithm.\n",
    "We will be estimating the following covariance function, described by Censi (Accurate closed-form estimation of ICP's covariance):\n",
    "$$\n",
    "  \\mathrm{cov}(X) = \\left(\\frac{\\partial^2 e}{\\partial X^2}\\right)^{-1} \\frac{\\partial^2 e}{\\partial X \\partial z} \n",
    "  \\mathrm{cov}(z) \\left( \\frac{\\partial^2 e}{\\partial X \\partial z} \\right)^T \\left(\\frac{\\partial^2 e}{\\partial X^2}\\right)^{-1}\n",
    "$$\n",
    "\n",
    "here $X= (x, y, \\theta)$ is a roto-translation reported by ICP to correct the robot's pose, $e$ error function used for minimization, which also depends on the measurement $z$. In implementation we use simple point to point ICP with error function:\n",
    "$$\n",
    "  e = \\sum_{ij} || q_i - Rp_j - t||^2  \\hspace{3cm}(1)\n",
    "    \\label{eq:errorFunc}\n",
    "$$\n",
    "\n",
    "where $p_j = [p_j^x, p_j^y]$ - a point in current scan, the one that should be transformed. These points should already be in a reference frame of the $q_i$; $q_i= [q_i^x, q_i^y]$ a corresponding point in reference scan, $R$ - rotation matrix and $t$ - translation reported by ICP:\n",
    "$$\n",
    "  R = \n",
    "  \\begin{pmatrix} \n",
    "    \\cos\\theta & -\\sin\\theta \\\\\n",
    "    \\sin\\theta & \\cos\\theta \n",
    "  \\end{pmatrix} \\hspace{2cm} \\mathrm{and} \\hspace{2cm}  \n",
    "  t = \n",
    "  \\begin{pmatrix} \n",
    "    x  \\\\\n",
    "    y \n",
    "  \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "Knowing that Eq. (1) represent the distance between 2 points, it can be rewritten in the following form:\n",
    "\\begin{equation}\n",
    "  e = \\sum_{ij}\\left(  q_i^x - (p_j^x \\cos\\theta - p_j^y\\sin\\theta) -x \\right)^2 +\n",
    "  \\left( q_i^y - (p_j^x\\sin\\theta + p_j^y\\cos\\theta) - y \\right)^2\n",
    "\\end{equation}\n",
    "\n",
    "Now let's see how to compute two main terms from the first equation $\\frac{\\partial^2 e}{\\partial X^2}$ and\n",
    "$\\frac{\\partial^2 e}{\\partial X \\partial z}$. The general formula for computing second order partial derivatives in form of Hessian has the following form:\n",
    "$$\n",
    "  \\frac{\\partial^2 e}{\\partial X^2} = \n",
    "    \\begin{pmatrix}\n",
    "    \\dfrac{\\partial^2 e}{\\partial x^2} &  \\dfrac{\\partial^2 e}{\\partial x \\partial y} &  \\dfrac{\\partial^2 e}{\\partial x \\partial \\theta} \\\\[3ex]\n",
    "    \\dfrac{\\partial^2 e}{\\partial y \\partial x} &  \\dfrac{\\partial^2 e}{\\partial y^2}  &  \\dfrac{\\partial^2 e}{\\partial y \\partial \\theta} \\\\[3ex]\n",
    "    \\dfrac{\\partial^2 e}{\\partial \\theta \\partial x} &  \\dfrac{\\partial^2 e}{\\partial \\theta \\partial y}  &  \\dfrac{\\partial^2 e}{\\partial \\theta^2}\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "Let's specify how the individual elements of this matrix should look like. We just need to take the respective derivatives over the $e$ function.\n",
    "\n",
    "\\begin{eqnarray}\n",
    "  \\dfrac{\\partial^2 e}{\\partial x^2}  &=& \\dfrac{\\partial^2 e}{\\partial y^2} = \\sum_{ij}2;  \\\\\n",
    "  \\dfrac{\\partial^2 e}{\\partial x \\partial y}  &=& \\dfrac{\\partial^2 e}{\\partial y \\partial x} = 0;  \\\\\n",
    "  \\dfrac{\\partial^2 e}{\\partial x \\partial \\theta} &=& \\dfrac{\\partial^2 e}{\\partial \\theta \\partial x} = \\sum_{ij} -2(c\\sin\\theta + d\\cos\\theta) \\\\\n",
    "  \\dfrac{\\partial^2 e}{\\partial y \\partial \\theta} &=& \\dfrac{\\partial^2 e}{\\partial \\theta \\partial y} = \\sum_{ij} \\left( 2c\\cos\\theta - 2d\\sin\\theta \\right)\\\\\n",
    "  \\dfrac{\\partial^2 e}{\\partial \\theta^2} &=& \\sum_{ij}  \\big( 2(c\\cos\\theta - d\\sin\\theta)(a - c\\cos\\theta + d\\sin\\theta - x) + \\\\\n",
    "    &+& 2(c\\sin\\theta + d\\cos\\theta)(b - c\\sin\\theta - d\\cos\\theta - y) + \\\\\n",
    "    &+& 2(c\\sin\\theta + d\\cos\\theta)^2 + 2(d\\sin\\theta - c\\cos\\theta)^2 \\big)\n",
    "\\end{eqnarray}\n",
    "\n",
    "**Note**. For simplicity of the notation I have renamed couple of variables, so $a = q_i^x$, $b = q_i^y$, $c = p_j^x$ and $d = p_j^y$.\n",
    "\n",
    "\n",
    "Now, the measurement $z$ represent all the points, that take part in the minimization procedure in the ICP. Meaning those $p_j$ that correspond to $q_i$. We consider that all points have some uncertainty associated to it and we want to take this uncertainty into account. If we consider to have a gaussian distribution centered in every point and that all of them have the same distribution, then $\\mathrm{cov}(z) = \\mathrm{cov}(q_1, q_2,...,q_N,p_1, p_2,...,p_N) = \\sigma_zI$, where $\\sigma_z$ - uncertainty about the individual measurement. This covariance is a $4N \\times 4N$ matrix, where N is the number of correspondences.\n",
    "\n",
    "**Question**: what will happen if some points in current scan $p_j$ have the same point correspondences in $q_i$. Should we include the $q_i$ point twice? Will the matrix then have linearly dependent rows? Full rank?\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And finally let's specify how the last term $\\dfrac{\\partial^2 e}{\\partial X \\partial z}$ will look like.\n",
    "\n",
    "$$\n",
    "  \\frac{\\partial^2 e}{\\partial X \\partial z} = \n",
    "    \\begin{pmatrix}\n",
    "    \\dfrac{\\partial^2 e}{\\partial x \\partial q^x_1} &  \\dfrac{\\partial^2 e}{\\partial x \\partial q^y_1} &\n",
    "    \\dfrac{\\partial^2 e}{\\partial x \\partial q^x_2} &  \\dfrac{\\partial^2 e}{\\partial x \\partial q^y_2} & ... &\n",
    "    \\dfrac{\\partial^2 e}{\\partial x \\partial p^x_1} & \\dfrac{\\partial^2 e}{\\partial x \\partial p^y_1}  & \n",
    "     \\dfrac{\\partial^2 e}{\\partial x \\partial p^x_2} & \\dfrac{\\partial^2 e}{\\partial x \\partial p^y_2}  & ... \\\\[3ex]\n",
    "\n",
    "    \\dfrac{\\partial^2 e}{\\partial y \\partial q^x_1} &  \\dfrac{\\partial^2 e}{\\partial y \\partial q^y_1} & \n",
    "    \\dfrac{\\partial^2 e}{\\partial y \\partial q^x_2} &  \\dfrac{\\partial^2 e}{\\partial y \\partial q^y_2} & ... &\n",
    "    \\dfrac{\\partial^2 e}{\\partial y \\partial p^x_1} & \\dfrac{\\partial^2 e}{\\partial y \\partial p^y_1}  & \n",
    "    \\dfrac{\\partial^2 e}{\\partial y \\partial p^x_2} & \\dfrac{\\partial^2 e}{\\partial y \\partial p^y_2}  & ... \\\\[3ex]\n",
    "    \\dfrac{\\partial^2 e}{\\partial \\theta \\partial q^x_1} &  \\dfrac{\\partial^2 e}{\\partial \\theta \\partial q^y_1} & \n",
    "    \\dfrac{\\partial^2 e}{\\partial \\theta \\partial q^x_2} &  \\dfrac{\\partial^2 e}{\\partial \\theta \\partial q^y_2} & ... &\n",
    "    \\dfrac{\\partial^2 e}{\\partial \\theta \\partial p^x_1} & \\dfrac{\\partial^2 e}{\\partial \\theta \\partial p^y_1}  &  \n",
    "    \\dfrac{\\partial^2 e}{\\partial \\theta \\partial p^x_2} & \\dfrac{\\partial^2 e}{\\partial \\theta \\partial p^y_2}  & ... \n",
    "\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "This matrix will be of dimension $3 \\times 2N$. After taking all the derivative the matrix will have the following pattern form:\n",
    "$$\n",
    "  \\frac{\\partial^2 e}{\\partial X \\partial z} = \n",
    "    \\begin{pmatrix}\n",
    "    -2 & 0 &-2 & 0 & ... & 2\\cos\\theta & -2\\sin\\theta & 2\\cos\\theta & -2\\sin\\theta & ...\\\\[3ex]\n",
    "    0 & -2 & 0 & -2 & ... & 2\\sin\\theta & 2\\cos\\theta & 2\\sin\\theta & 2\\cos\\theta & ...\\\\[3ex]\n",
    "    A_1 & B_1 & A_2 & B_2 & ... & C_1 & D_1 & C_2 & D_2 & ...\n",
    "\n",
    "    \\end{pmatrix}\n",
    "$$\n",
    "\n",
    "where \n",
    "$$\n",
    "  A_k &=& 2(p^x_k\\sin\\theta + p^y_k \\cos\\theta) \\\\ \n",
    "  B_k &=& 2(p^y_k\\sin\\theta - p^x_k\\cos\\theta) \\\\\n",
    "  C_k &=& 2\\sin\\theta(q^x_k-p^x_k\\cos\\theta + p^y_k\\sin\\theta - x) - 2\\cos\\theta(q^y_k - p^x_k\\sin\\theta - p^y_k\\cos\\theta - y) - \\\\\n",
    "    &-& 2\\cos\\theta(p^x_k\\sin\\theta + p^y_k\\cos\\theta) - 2\\sin\\theta(p^y_k\\sin\\theta - p^x_k\\cos\\theta) \\\\ \n",
    "  D_k &=& 2\\cos\\theta(q^x_k - p^x_k\\cos\\theta + p^y_k\\sin\\theta - x) + 2\\cos\\theta(q^y_k -p^x_k\\sin\\theta - p^y_k\\cos\\theta - y) + \\\\\n",
    "    &+& 2\\sin\\theta(p^x_k\\sin\\theta + p^y_k\\cos\\theta) - 2\\cos\\theta(p^y_k\\sin\\theta - p^x_k\\cos\\theta)\n",
    "$$\n",
    "\n",
    "Now we have derived all the necessary steps for programming. If it will work I will be really proud of myself!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
